#include "DriveSystem.h"
#include "Net.h"

DriveSystem* DriveSystem::_thisInstance = NULL;

DriveSystem::DriveSystem(DriverConsole* console, NetworkTable *smartDash) {
	printf("Initializing drive system\n");
	_motorFL = new MotorWrapper(MOTOR_FL_CHAN,MotorWrapper::speed,false);
	_motorFL->SetEncoderCPP(ENC_PER_REV);
	_motorFR = new MotorWrapper(MOTOR_FR_CHAN,MotorWrapper::speed,false);
	_motorFR->SetEncoderCPP(ENC_PER_REV);
	_motorBL = new MotorWrapper(MOTOR_BL_CHAN,MotorWrapper::speed,false);
	_motorFR->SetEncoderCPP(ENC_PER_REV);
	_motorBR = new MotorWrapper(MOTOR_BR_CHAN,MotorWrapper::speed,true);
	_motorBR->SetEncoderCPP(ENC_PER_REV);
	/*_gyro = new Gyro(GYRO_CHANNEL);
	_awenc = new AllWheelEncoder(_motorFL,_motorFR,_motorBL,_motorFR);
	_linPID = new LinearPID(_motorFL,_motorFR,_motorBL,_motorFR);
	_turnPID = new TurnPID(_motorFL,_motorFR,_motorBL,_motorFR);
	_turnPIDController = new PIDController(TURN_PID_P,TURN_PID_I,TURN_PID_D,_gyro,_turnPID);
	_turnPIDController->Disable();
	_linPIDController = new PIDController(LIN_PID_P,LIN_PID_I,LIN_PID_D,_awenc,_linPID);
	_linPIDController->Disable();*/
	_driverConsole = console;
	_smartDash = smartDash;
}

DriveSystem::~DriveSystem() {
	delete _motorFL;
	delete _motorFR;
	delete _motorBL;
	delete _motorBR;
}

DriveSystem* DriveSystem::GetInstance(DriverConsole* console, NetworkTable *smartDash) {
	if (NULL == _thisInstance)
		_thisInstance = new DriveSystem(console, smartDash);
	return _thisInstance;
}

void DriveSystem::Teleop(DriveMode mode) {
	//_turnPIDController->Disable();
	//_linPIDController->Disable();
	if (mode == tank) {
		float yValOne = _driverConsole->GetJoystickY(DriverConsole::JoystickLeft);
		float yValTwo = _driverConsole->GetJoystickY(DriverConsole::JoystickRight);
		yValOne = Limit(yValOne);
		yValOne = SignSquare(yValOne);
		yValTwo = Limit(yValTwo);
		yValTwo = SignSquare(yValTwo);
		SetMotorSpeeds(yValOne,yValTwo,yValOne,yValTwo);
	} else if (mode == arcade) {
		float xVal = _driverConsole->GetJoystickX(DriverConsole::JoystickRight);
		float yVal = _driverConsole->GetJoystickY(DriverConsole::JoystickRight);
		xVal = Limit(xVal);
		yVal = Limit(yVal);
		xVal = SignSquare(xVal);
		yVal = SignSquare(yVal);
		float left = yVal + xVal;
		float right = yVal - xVal;
		left = Limit(left);
		right = Limit(right);
		SetMotorSpeeds(left,right,left,right);
	}
}

void DriveSystem::MoveForward(float dist) {
	_linPIDController->Enable();
	_linPIDController->SetSetpoint(dist);
}

void DriveSystem::Turn(float angle) {
	_turnPIDController->Enable();
	_turnPIDController->SetSetpoint(angle);
}

float DriveSystem::Limit(float num) {
	if (num > 1)
		num = 1;
	else if (num < -1) 
		num = -1;
	return num;
}

float DriveSystem::SignSquare(float num) {
	return num*fabs(num);
}

void DriveSystem::SetMotorSpeeds(float speedFL, float speedFR, float speedBL, float speedBR) {
	_motorFL->SetSetpoint(MAX_RPM*speedFL);
	_motorFR->SetSetpoint(MAX_RPM*speedFR);
	_motorBL->SetSetpoint(MAX_RPM*speedBL);
	_motorBR->SetSetpoint(MAX_RPM*speedBR);
}

void DriveSystem::UpdateFromDash() {
	//should call this along with DashPost()
}

void DriveSystem::DashPost() {
	//printf("FL Position : %.2f\n", _motorFL->GetState());
	//printf("FR osition : %.2f\n", _motorFR->GetState());
	//printf("BL sdflkj : %.2f\n", _motorBL->GetState());
	//printf("BR alsdkjf : %.2f\n", _motorBR->GetState());
	/*
	printf("Posting to the SmartDashboard...\n");
	//First post the current speed
	_smartDash->PutDouble(SD_MOTOR_FL_SPEED, Round(_motorFL->GetState(),2));
	_smartDash->PutDouble(SD_MOTOR_FR_SPEED, Round(_motorFR->GetState(),2));
	_smartDash->PutDouble(SD_MOTOR_BL_SPEED, Round(_motorBL->GetState(),2));
	_smartDash->PutDouble(SD_MOTOR_BR_SPEED, Round(_motorBR->GetState(),2));
	
	//Now post all of the setpoints
	_smartDash->PutDouble(SD_MOTOR_FL_SETPT, Round(_motorFL->GetSetpoint(),2));
	_smartDash->PutDouble(SD_MOTOR_FR_SETPT, Round(_motorFR->GetSetpoint(),2));
	_smartDash->PutDouble(SD_MOTOR_BL_SETPT, Round(_motorBL->GetSetpoint(),2));
	_smartDash->PutDouble(SD_MOTOR_BR_SETPT, Round(_motorBR->GetSetpoint(),2));
	
	//now post the current PID constants
	//assume all the same, but fine since all changed the same
	_smartDash->PutData(SD_PID_FL, _motorFL->GetPIDInstance());
	_smartDash->PutData(SD_PID_FR, _motorFR->GetPIDInstance());
	_smartDash->PutData(SD_PID_BL, _motorBL->GetPIDInstance());
	_smartDash->PutData(SD_PID_BR, _motorBR->GetPIDInstance());
	*/
}
float DriveSystem::Round(float num, int numPlaces){
	int numForPlacesInt;
	float numForPlaces;
	numForPlaces = pow(10, numPlaces) * num;
	numForPlacesInt = (int) numForPlaces;
	if (numForPlaces - numForPlacesInt < .5) return ((float)numForPlacesInt / pow(10, numPlaces));
	else return (((float)numForPlacesInt + 1) / pow(10, numPlaces));
}
